package pl.gigiel.behaviour.steer;

import pl.gigiel.behaviour.IBoid;
import pl.gigiel.behaviour.IBoidFeatures;
import pl.gigiel.behaviour.ISteer;
import pl.gigiel.behaviour.exception.BehaviourException;

import com.jme.math.Vector3f;

public class SeparationSteer implements ISteer {
	private static ISteer steer;

	private SeparationSteer() {
	}

	public static ISteer getInstance() {
		if (steer == null) {
			steer = new SeparationSteer();
		}
		return steer;
	}

	@Override
	public void init() throws BehaviourException {
		// nothing
	}

	@Override
	public Vector3f force(IBoid boid, float elapsed) throws BehaviourException {
		Vector3f result = new Vector3f();
		IBoidFeatures features = boid.getFeatures();
		Vector3f position = features.getPosition();
		for (IBoid neighbour : boid.getEnvironment().getNeighbours(boid)) {
			Vector3f toAgent = position.subtract(neighbour.getFeatures().getPosition());
			float length = toAgent.length();
			Vector3f offset = toAgent.normalizeLocal().divide(length);
			result.addLocal(offset);
		}
		result = correctedLocal(result);
		return result;
	}

	// very rare situation:
	// 1. boids are in the same place -> NaN -> separation force is
	// POSITIVE_INFINITY
	private Vector3f correctedLocal(Vector3f vector) {
		if (Float.isNaN(vector.x)) {
			vector.x = Float.POSITIVE_INFINITY;
		}
		if (Float.isNaN(vector.y)) {
			vector.y = Float.POSITIVE_INFINITY;
		}
		if (Float.isNaN(vector.z)) {
			vector.z = Float.POSITIVE_INFINITY;
		}
		return vector;
	}
}
